'''
Created on Jul 12, 2009

@author: bryan
'''
#===============================================================================
# Imports
#===============================================================================
import Optimize
import numpy as np
from numpy import pi
from scipy import optimize
import pylab

class Autotune():
    """
    Use optimization to find best parameter set for regisration
    """
    
    def __init__(self):
        """
        Constructor
        """
        pass
    
    def run(self):
        """
        """
        reg_optimizer = Optimize.Optimize()
        reg_optimizer.Setup()
        
#===============================================================================
#        Setup logging
#===============================================================================
        logTag = 'Autotune'     #This tag can be used to identify trials
        logName = time.strftime('%Y%m%d_%H%M%S')+'_'+logTag+'.log'
        logDir = os.path.join(os.getcwd(),'log')
        print 'Logging dir: ', logDir  
        try:
            os.mkdir(logDir)
        except:
            pass
        logFile = csv.writer(open(os.path.join(logDir,logName),'wt'),
                                   dialect='excel-tab')
        # Write Headers
        logFile.writerow(('Iteration','Cost','Pose0(mm)','Pose1(mm)','Pose2(mm)',
                                'Pose3(deg)','Pose4(deg)','Pose5(deg)','BestCost'))
        
#===============================================================================
#        Brute force optimization
#===============================================================================
        trans_range = np.arange(.1,2.1,.1)
        rot_range = np.arange(.2,4.2,.2)*pi/180 # scale range to radians
        err = np.zeros((len(trans_range),len(rot_range)))
        for i in trans_range:
            for j in rot_range:
                search_range = np.asarray([i,i,i,j,j,j])
                err[i,j] = reg_optimizer.SimulatedAnnealing(search_range)
                print 'Search Range: ', search_range, ', Error: ', err[i,j]
        
        return err
            
#        (xmin, Jmin, T, feval, iters, accept, retval) = optimize.anneal(
#                                    f, x0, args=(), 
#                                    full_output=True, 
#                                    T0=1, Tf=1e-10, lower=lowerBounds, 
#                                    upper=upperBounds, m=1, n=1,
#                                    maxiter=10, maxeval=10, 
#                                    maxaccept=50, dwell=10, 
#                                    feps=1e-3, schedule='fast',
#                                    boltzmann=1e-5, quench=1)

if __name__ == '__main__':
    tune = Autotune()
    err = tune.run()
    pylab.plot(err)
    pylab.imshow(err)
    